#include <ros/ros.h>
#include <std_msgs/String.h>
#include <mavros_msgs/RCIn.h>
#include <uav/servo.h>




int is_deliver[3]={1,1,1};
int servo_num=0;

void rcCallback(const mavros_msgs::RCIn::ConstPtr& rcmsg)
{
	printf("channels: ");
	for (int i = 0; i < 8; i++)
		printf("[%d] ", rcmsg->channels[i]);
	printf("\r");
}

void numCallback(const mavros_msgs::RCIn::ConstPtr& rcmsg)
{
	int ch1 = rcmsg->channels[6];
	int ch2 = rcmsg->channels[7];
	int servo=-1;

	if (ch1 <= 1100 && ch2<=1100)
	{
		servo = -1;
		//is_deliver={1,1,1};
	}
	if (ch1>1900 && ch2<1100)
	{
		servo = 0;
	}
	if (ch1<1100 && ch2>1900)
	{
		servo = 1;
	}
	if (ch1>1900 && ch2>1900)
	{
		servo = 2;
    }
	
	//printf("[%d,%d]\r", ch1,ch2);
    // 舵机控制
	if (servo >= 0) {

		if (is_deliver[0] && servo==0) {
			is_deliver[0] = 0;
			servo_num=33;
		}
		else if (is_deliver[1] && servo==1) {
			is_deliver[1] = 0;
			servo_num=35;
		}
		else if (is_deliver[2] && servo==2) {
			is_deliver[2] = 0;
			servo_num=37;
		}
	}
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "rc_sub");
	ros::NodeHandle n;


	ros::Subscriber sub = n.subscribe("/mavros/rc/in", 100, numCallback);
	ros::Publisher servo_pub = n.advertise<uav::servo>("/uav/servo", 10);

	uav::servo servo_info;
	servo_info.direction=1;
	servo_info.id=servo_num;

	ros::Rate rate(20);
	while (ros::ok())
	{
		if (servo_info.id != servo_num)
		{
			servo_info.id=servo_num;
			servo_pub.publish(servo_info);
			ROS_INFO("servo:%d",servo_info.id);
		}
		rate.sleep();
		ros::spinOnce();
	};
	
	return 0; 
}
